/*
 * Created on 18 novembre 2005
 * bredeche(at)lri.fr
 *
 * This is demo for loading a map from an image file in Simbad . 
 * 
 * The important parts are:
 * - the main class, which describes the environment and setup the robot
 * - the robot class, which is embedded in the latter
 *  
 */

package piconode.tutorials;

import java.io.FileNotFoundException;
import java.io.FileReader;
import java.util.ArrayList;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import piconode.core.node.FeedForwardNeuralNetwork;
import piconode.factory.MultiLayerPerceptronFactory;
import simbad.gui.Simbatch;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;
import contribs.maploader.SimpleImage;

/**
 * This class represents an Environment useable in Simbad but with the
 * specification of being defined by a PNG image. For the moment, the image is
 * defined as follows - 20x20 pixels (have to fit) - a red (rgb 255,0,0) pixel
 * means a box - a green (rgb 0,255,0) pixel represent a goal to reach (if any) -
 * a blue (rgb 0,0,255) pixel represents the starting position - dark gray (rgb
 * 60,60,60) pixel representing a simulated ball - the image have to be in a
 * PNG, JPG or GIF format.
 * 
 * TODO : - define more flexible constraints (20x20 -> ~ [10:50]x[10;50]) or
 * something like that (for now : ([0:20],[0:20]) values but it should be more
 * likely ([10:20],[10:20])) - add a niveler due to the rgb values for more
 * visual effects - add the specific (table) walls defined by some intermediary
 * color.
 * 
 * 
 * ADDED : - multi floor : example : if you load image.png, the app will search
 * for image-2.png for the second floor, image-3.png for the third, etc. -
 * Multi-robot instances
 * 
 * @author nicolas (+ cedric)
 * 
 */

public class Tutorial_6b_Robot_EvaluatingRandomControllers extends EnvironmentDescription {

	// static private Simbatch sim;

	// a goal to reach
	public Point3d _goal = new Point3d();
	// the initial starting points for robots (contains Point3d)
	public ArrayList _startingPoints = new ArrayList();

	ArrayList<Robot> robots = new ArrayList<Robot>();

	public Tutorial_6b_Robot_EvaluatingRandomControllers() {
		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);

		// create the robot

		Robot robot = new Robot(new Vector3d(0, 0, 0), "MapRobot");
		this.robots.add(robot);
		add(robot);

	}

	/**
	 * initialize the environment due to the file __filename. TODO : - create
	 * multi-objectives points - create balls or non static items.
	 * 
	 * @param __filename
	 */
	public Tutorial_6b_Robot_EvaluatingRandomControllers(String __filename) {
		int[] values;

		// initialise the image
		SimpleImage simpleImage = new SimpleImage(__filename, false);
		simpleImage.displayInformation();

		// step 2 : for each data in the image, initialize the environment
		// + starting position (for one or more robots)
		// + goal position
		for (int y = 0; y != simpleImage.getHeight(); y++) {
			System.out.print(" ");
			for (int x = 0; x != simpleImage.getWidth(); x++) {
				values = simpleImage.getPixel(x, y);
				if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
					// red value, we will display a Wall in here :
					add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
					System.out.print("#");
				} else if (values[1] < 50 && values[2] > 200 && values[3] < 50) {
					// green value : define the goal point
					_goal.x = x;
					_goal.z = y;
					_goal.y = 0;
					System.out.print("X");
				} else if (values[1] < 50 && values[2] < 50 && values[3] > 200) {
					_startingPoints.add(new Point3d(x, 0, y));
					// starting position
					System.out.print("!");
				} else if (values[1] < 100 && values[1] == values[2] && values[2] == values[3]) {
					// add a ball
					// take care because the setUsePhysics remove the
					// agentInspector
					showAxis(false);
					setUsePhysics(true);
					add(new BallAgent(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), "ball", new Color3f(200, 0, 0), 0.25f, 0.25f));

				} else
					System.out.print(" ");
			}
			System.out.print("\n");
		}

		String secondFloor = __filename;
		boolean hasNextFloor = true;
		int cpt = 2;
		// add other floors to the environment
		// the other files should be called : for example if the initial file is
		// maze.png
		// - maze-2.png, maze-3.png, ....
		while (hasNextFloor) {
			try {
				// step 3 : define a second floor if exists
				if (__filename.endsWith(".png"))
					secondFloor = __filename.replaceAll(".png", "-" + cpt + ".png");
				if (__filename.endsWith(".gif"))
					secondFloor = __filename.replaceAll(".gif", "-" + cpt + ".gif");
				if (__filename.endsWith(".jpg"))
					secondFloor = __filename.replaceAll(".jpg", "-" + cpt + ".jpg");
				// only way found to check if the file exist
				// if it does not exists an exception is raised and we can
				// poursue without adding a second floor.
				new FileReader(secondFloor);

				// step 4 : initialise the image
				simpleImage = new SimpleImage(secondFloor, false);
				simpleImage.displayInformation();

				// step 5 : for each data in the image, update the environment
				for (int y = 0; y != simpleImage.getHeight(); y++) {
					System.out.print(" ");
					for (int x = 0; x != simpleImage.getWidth(); x++) {
						values = simpleImage.getPixel(x, y);
						if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
							// red value, we will display a Wall in here :
							add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), cpt - 1, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
							System.out.print("#");
						} else
							System.out.print(" ");
					}
					System.out.print("\n");
				}
				cpt++;
			} catch (FileNotFoundException fnfe) {
				// do nothing : do not add a second floor
				if (cpt == 2) {
					System.out.println("no second floor.");
					System.out.println(" - to define a second floor, create a file called : " + secondFloor);
					System.out.println("");
				}
				hasNextFloor = false;
			}
		}

		// step 4 : add the robots of Robot instances
		for (int i = 0; i < _startingPoints.size(); i++) {
			Robot robot = new Robot(new Vector3d(((Point3d) _startingPoints.get(i)).x - (simpleImage.getWidth() / 2), 0f, ((Point3d) _startingPoints.get(i)).z - (simpleImage.getHeight() / 2)), "robot n." + i, this);
			// add(new Robot(new
			// Vector3d(((Point3d)_startingPoints.get(i)).x-(simpleImage.getWidth()/2),
			// 0f,
			// ((Point3d)_startingPoints.get(i)).z-(simpleImage.getHeight()/2)),
			// "openDProbot"));
			this.robots.add(robot);
			add(robot);

		}
	}

	public void resetRobots() {
		for (int i = 0; i != this.robots.size(); i++) {
			this.robots.get(i).moveToStartPosition();
			this.robots.get(i).setRunning(true);
		}
	}

	public Robot getRobot(int __index) {
		return this.robots.get(__index);
	}

	// the robot is defined as an embedded class
	public class Robot extends Agent {

		EnvironmentDescription _environment;

		RangeSensorBelt sonars, bumpers;
		FeedForwardNeuralNetwork _ctl;

		boolean _running = true;

		public Robot(Vector3d position, String name) {
			super(position, name);

			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this, 8);
			bumpers = RobotFactory.addBumperBeltSensor(this, 12);
		}

		public Robot(Vector3d position, String name, EnvironmentDescription env) {
			super(position, name);

			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this, 8);
			bumpers = RobotFactory.addBumperBeltSensor(this, 12);

			this._environment = env;
		}

		/** Initialize Agent's Behavior */
		@Override
		public void initBehavior() {
			// nothing particular in this case
		}

		/** set new controller */
		public void setController(FeedForwardNeuralNetwork __ctl) {
			if (__ctl == null) {
				System.err.println("[error] controller cannot be set");
				System.exit(-1);
			}
			this._ctl = __ctl;
		}

		/** Perform one step of Agent's Behavior */
		@Override
		public void performBehavior() {
			if (this._ctl == null) {
				System.err.println("[error] no controller");
				System.exit(-1);
			}

			if (bumpers.oneHasHit() || collisionDetected()) {
				// stop the robot
				setTranslationalVelocity(0.0);
				setRotationalVelocity(0);
				this._running = false;
			} else {
				// perform one step
				double[] inputs = new double[4];
				inputs[0] = sonars.getFrontLeftQuadrantMeasurement();
				inputs[1] = sonars.getFrontRightQuadrantMeasurement();
				inputs[2] = sonars.getFrontQuadrantMeasurement();
				inputs[3] = sonars.getBackQuadrantMeasurement();
				this._ctl.step();
				setTranslationalVelocity(this._ctl.getOutputNeuronAt(0).getValue());
				setRotationalVelocity(this._ctl.getOutputNeuronAt(1).getValue());
			}

			/*
			 * // *** clue-less robot *** if (collisionDetected()) { // stop the
			 * robot setTranslationalVelocity(0.0); setRotationalVelocity(0); }
			 * else { setTranslationalVelocity(1.5-2*Math.random());
			 * setRotationalVelocity(0.5-Math.random()); }
			 */

			// *** avoider robot ***
			/*
			 * if (bumpers.oneHasHit()) { setTranslationalVelocity(-0.1);
			 * setRotationalVelocity(0.5-(0.1 * Math.random())); } else if
			 * (collisionDetected()) { // stop the robot
			 * setTranslationalVelocity(0.0); setRotationalVelocity(0); } else
			 * if (sonars.oneHasHit()) { // reads the three front quadrants
			 * double left = sonars.getFrontLeftQuadrantMeasurement(); double
			 * right = sonars.getFrontRightQuadrantMeasurement(); double front =
			 * sonars.getFrontQuadrantMeasurement(); // if obstacle near if
			 * ((front < 0.7)||(left < 0.7)||(right < 0.7)) { if (left < right)
			 * setRotationalVelocity(-1); else setRotationalVelocity(1);
			 * setTranslationalVelocity(0); } else{ setRotationalVelocity(0);
			 * setTranslationalVelocity(0.6); } } else {
			 * setTranslationalVelocity(0.8);; setRotationalVelocity(0); }
			 */

		}

		public void setRunning(boolean __flag) {
			this._running = __flag;
		}

		public boolean getRunning() {
			return this._running;
		}

	}

	/*
	 * for test purposes
	 */
	public static void main(String[] args) {

		// fast method with simulation relaunch (e.g. useful when evolving) --
		// every new individual has random weights

		Tutorial_6b_Robot_EvaluatingRandomControllers environment = new Tutorial_6b_Robot_EvaluatingRandomControllers("piconode/tutorials/robotmap.png");
		Simbatch sim = new Simbatch(environment, true);

		FeedForwardNeuralNetwork ctl = MultiLayerPerceptronFactory.createPerceptron(4, 5, 2, false);
		double[] weights = new double[ctl.getNumberOfAllArcs()];
		environment.getRobot(0).setController(ctl);

		int k = 0;
		int maxEval = 1000; // number of random robots to be tested

		while (k < maxEval) // launch several evaluations.
		{
			System.out.println("beginning of evaluation no." + (k + 1));

			// reset simulator and environment
			sim.reset();
			environment.resetRobots();

			// load genome
			for (int i = 0; i != ctl.getNumberOfAllArcs(); i++)
				weights[i] = 2. * Math.random() - 1.;
			ctl.setAllArcsWeightValues(weights);

			// run robot
			int i = 0;
			while (i < 1000 && environment.getRobot(0).getRunning() == true) {
				// System.out.print(".");
				sim.step();
				i++;
			}
			// System.out.println("");
			System.out.println("end of evaluation no." + (k + 1));
			k++;
		}
		/**/
	}
}